#include "dexterous_hand.h"
#include <iostream>
#include <iomanip>
#include <sstream>

DexterousHand::DexterousHand(const std::string& port, unsigned long baud)
    : port_(port), baud_(baud), control_frequency_(50) {
    // 初始化下行数据
    memset(&downlink_data_, 0, sizeof(downlink_data_));
    memset(&uplink_data_, 0, sizeof(uplink_data_));
}

DexterousHand::~DexterousHand() {
    close();
}

bool DexterousHand::initialize() {
    try {
        serial_ = std::make_unique<serial::Serial>(port_, baud_, 
                  serial::Timeout::simpleTimeout(1000));
        
        if (!serial_->isOpen()) {
            std::cerr << "无法打开串口: " << port_ << std::endl;
            return false;
        }
        
        connected_.store(true);
        std::cout << "串口 " << port_ << " 连接成功" << std::endl;
        return true;
        
    } catch (const std::exception& e) {
        std::cerr << "串口初始化错误: " << e.what() << std::endl;
        return false;
    }
}

void DexterousHand::close() {
    stopRealTimeControl();
    connected_.store(false);
    
    if (serial_ && serial_->isOpen()) {
        serial_->close();
    }
}

bool DexterousHand::startRealTimeControl(int frequency_hz) {
    if (!connected_.load()) {
        std::cerr << "设备未连接" << std::endl;
        return false;
    }
    
    if (real_time_running_.load()) {
        std::cout << "实时控制已经在运行" << std::endl;
        return true;
    }
    
    control_frequency_ = frequency_hz;
    real_time_running_.store(true);
    
    real_time_thread_ = std::make_unique<std::thread>(&DexterousHand::realTimeControlLoop, this);
    
    std::cout << "实时控制启动，频率: " << frequency_hz << "Hz" << std::endl;
    return true;
}

void DexterousHand::stopRealTimeControl() {
    real_time_running_.store(false);
    
    if (real_time_thread_ && real_time_thread_->joinable()) {
        real_time_thread_->join();
    }
    
    std::cout << "实时控制已停止" << std::endl;
}

void DexterousHand::setJointParams(int joint_id, const JointParamsDownlink& params) {
    if (joint_id < 0 || joint_id >= 21) {
        std::cerr << "关节ID超出范围: " << joint_id << std::endl;
        return;
    }
    
    std::lock_guard<std::mutex> lock(data_mutex_);
    downlink_data_.jointParams[joint_id] = params;
}

JointParamsUplink DexterousHand::getJointStatus(int joint_id) const {
    if (joint_id < 0 || joint_id >= 21) {
        std::cerr << "关节ID超出范围: " << joint_id << std::endl;
        return {};
    }
    
    std::lock_guard<std::mutex> lock(data_mutex_);
    return uplink_data_.jointParams[joint_id];
}

SensorParam DexterousHand::getSensorData(int sensor_id) const {
    if (sensor_id < 0 || sensor_id >= 18) {
        std::cerr << "传感器ID超出范围: " << sensor_id << std::endl;
        return {};
    }
    
    std::lock_guard<std::mutex> lock(data_mutex_);
    return uplink_data_.sensorParams[sensor_id];
}

bool DexterousHand::getHandParameters(UserData& user_data) {
    std::vector<uint8_t> response_data;
    
    if (!sendCommand(Protocol::CMD_GET_HAND_PARAMS)) {
        return false;
    }
    
    if (!receiveResponse(Protocol::CMD_GET_HAND_PARAMS, response_data)) {
        return false;
    }
    
    if (response_data.size() >= sizeof(UserData)) {
        memcpy(&user_data, response_data.data(), sizeof(UserData));
        return true;
    }
    
    return false;
}

void DexterousHand::setStatusCallback(std::function<void(const RS485Uplink&)> callback) {
    status_callback_ = callback;
}

// 获取最新状态（线程安全）
RS485Uplink DexterousHand::getHandStatus() const {
    std::lock_guard<std::mutex> lock(data_mutex_);
    return uplink_data_;
}

void DexterousHand::realTimeControlLoop() {
    auto period = std::chrono::microseconds(1000000 / control_frequency_);
    auto next_time = std::chrono::steady_clock::now();
    
    while (real_time_running_.load()) {
        // 发送控制命令
        {
            std::lock_guard<std::mutex> lock(data_mutex_);
            if (!sendCommand(Protocol::CMD_REAL_TIME_CONTROL, 
                           &downlink_data_, sizeof(downlink_data_))) {
                std::cerr << "发送实时控制命令失败" << std::endl;
            }
        }
        
        // 处理接收数据
        processReceivedData();
        
        // 等待下一个周期
        next_time += period;
        std::this_thread::sleep_until(next_time);
    }
}

bool DexterousHand::sendCommand(uint8_t cmd, const void* data, uint16_t data_len) {
    if (!connected_.load() || !serial_) {
        return false;
    }
    
    try {
        Packet packet;
        packet.buildPacket(Protocol::HOST_ID, hand_id_, cmd, data, data_len);
        
        const auto& packet_data = packet.getData();
        size_t written = serial_->write(packet_data);
        
        return written == packet_data.size();
        
    } catch (const std::exception& e) {
        std::cerr << "发送命令错误: " << e.what() << std::endl;
        return false;
    }
}

bool DexterousHand::receiveResponse(uint8_t expected_cmd, std::vector<uint8_t>& response_data, 
                                   int timeout_ms) {
    if (!connected_.load() || !serial_) {
        return false;
    }
    
    try {
        auto start_time = std::chrono::steady_clock::now();
        std::vector<uint8_t> buffer;
        
        while (std::chrono::duration_cast<std::chrono::milliseconds>(
               std::chrono::steady_clock::now() - start_time).count() < timeout_ms) {
            
            size_t available = serial_->available();
            if (available > 0) {
                std::string recv = serial_->read(available);
                buffer.insert(buffer.end(), recv.begin(), recv.end());
                
                // 尝试解析数据包
                if (buffer.size() >= 9) { // 最小包长度
                    Packet packet;
                    uint8_t src_addr, dst_addr, cmd;
                    
                    if (packet.parsePacket(buffer, src_addr, dst_addr, cmd, response_data)) {
                        if (cmd == expected_cmd && src_addr == hand_id_ && 
                            dst_addr == Protocol::HOST_ID) {
                            return true;
                        }
                    }
                }
            }
            
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
        
    } catch (const std::exception& e) {
        std::cerr << "接收响应错误: " << e.what() << std::endl;
    }
    
    return false;
}

void DexterousHand::processReceivedData() {
    if (!connected_.load() || !serial_) {
        return;
    }
    
    try {
        size_t available = serial_->available();
        if (available > 0) {
            std::string recv = serial_->read(available);
            std::vector<uint8_t> buffer(recv.begin(), recv.end());
            
            Packet packet;
            uint8_t src_addr, dst_addr, cmd;
            std::vector<uint8_t> payload;
            
            if (packet.parsePacket(buffer, src_addr, dst_addr, cmd, payload)) {
                if (cmd == Protocol::CMD_REAL_TIME_CONTROL && 
                    src_addr == hand_id_ && dst_addr == Protocol::HOST_ID) {
                    
                    if (payload.size() >= sizeof(RS485Uplink)) {
                        std::lock_guard<std::mutex> lock(data_mutex_);
                        memcpy(&uplink_data_, payload.data(), sizeof(RS485Uplink));
                        
                        // 调用状态回调
                        if (status_callback_) {
                            status_callback_(uplink_data_);
                        }
                    }
                }
            }
        }
        
    } catch (const std::exception& e) {
        std::cerr << "处理接收数据错误: " << e.what() << std::endl;
    }
}

std::string DexterousHand::getCurrentTimeString() {
    using namespace std::chrono;
    auto now = system_clock::now();
    auto t = system_clock::to_time_t(now);
    auto ms = duration_cast<milliseconds>(now.time_since_epoch()) % 1000;

    std::tm tm_now;
#if defined(_WIN32)
    localtime_s(&tm_now, &t);
#else
    localtime_r(&t, &tm_now);
#endif

    std::ostringstream oss;
    oss << std::put_time(&tm_now, "%Y-%m-%d %H:%M:%S")
        << '.' << std::setfill('0') << std::setw(3) << ms.count();
    return oss.str();
}